﻿/********************************************************
 *  ██████╗  ██████╗████████╗██╗
 * ██╔════╝ ██╔════╝╚══██╔══╝██║
 * ██║  ███╗██║        ██║   ██║
 * ██║   ██║██║        ██║   ██║
 * ╚██████╔╝╚██████╗   ██║   ███████╗
 *  ╚═════╝  ╚═════╝   ╚═╝   ╚══════╝
 * Geophysical Computational Tools & Library (GCTL)
 *
 * Copyright (c) 2023  Yi Zhang (yizhang-geo@zju.edu.cn)
 *
 * GCTL is distributed under a dual licensing scheme. You can redistribute 
 * it and/or modify it under the terms of the GNU Lesser General Public 
 * License as published by the Free Software Foundation, either version 2 
 * of the License, or (at your option) any later version. You should have 
 * received a copy of the GNU Lesser General Public License along with this 
 * program. If not, see <http://www.gnu.org/licenses/>.
 * 
 * If the terms and conditions of the LGPL v.2. would prevent you from using 
 * the GCTL, please consider the option to obtain a commercial license for a 
 * fee. These licenses are offered by the GCTL's original author. As a rule, 
 * licenses are provided "as-is", unlimited in time for a one time fee. Please 
 * send corresponding requests to: yizhang-geo@zju.edu.cn. Please do not forget 
 * to include some description of your company and the realm of its activities. 
 * Also add information on how to contact you by electronic and paper mail.
 ******************************************************/

#include "math.h"

double gctl::sign(double a)
{
	if (a > GCTL_ZERO) return 1.0;
	if (a < -1.0*GCTL_ZERO) return -1.0; 
	return 0.0;
}

//利用二分法求一个正数的n次方根 注意输入值小于1的情况
double gctl::sqrtn(double d,int n,double eps)
{
	double xmin,xmax,halfx;
	if (d == 1)
	{
		return d;
	}
	else if (d > 1)
	{
		xmin = 1;
		xmax = d;
		halfx = 0.5*(xmin+xmax);
		while (fabs(d - pow(halfx,n)) > eps)
		{
			if (pow(halfx,n) > d)
			{
				xmax = halfx;
				halfx = 0.5*(xmin+xmax);
			}
			else
			{
				xmin = halfx;
				halfx = 0.5*(xmin+xmax);
			}
		}
	}
	else
	{
		xmin = 0;
		xmax = 1;
		halfx = 0.5*(xmin+xmax);
		while (fabs(d - pow(halfx,n)) > eps)
		{
			if (pow(halfx,n) > d)
			{
				xmax = halfx;
				halfx = 0.5*(xmin+xmax);
			}
			else
			{
				xmin = halfx;
				halfx = 0.5*(xmin+xmax);
			}
		}
	}
	return halfx;
}

/**
 * @brief      计算一个椭圆在不同位置的半径
 *
 * @param[in]  x_len  x方向半径
 * @param[in]  y_len  y方向半径
 * @param[in]  x_arc  椭圆绕原点逆时针旋转的角度 （弧度）
 * @param[in]  arc    计算方向与x轴正方向的夹角（弧度） 逆时针为正
 *
 * @return     半径值
 */
double gctl::ellipse_radius_2d(double x_len, double y_len, double arc, double x_arc)
{
	if (fabs(x_len - y_len) < 1e-8) // 就是个圆 直接加
	{
		return 0.5*(x_len + y_len);
	}
	
	return sqrt(pow(x_len*cos(arc-x_arc),2) + pow(y_len*sin(arc-x_arc),2));
}

/**
 * @brief     计算已椭圆为基准面的高程数据的绝对坐标位置。
 * 
 * 假设高程数据的测量方向（如大地水准面）为椭圆切线的垂直方向（与切点与坐标原点的连线方向不一致）
 * 则高程点的绝对空间位置的维度值与切点的维度值也不一致。此函数可以计算校正后的维度值与球心半径。
 * 
 * @param x_len     椭圆的x方向半径（一般为长轴）
 * @param y_len     椭圆的y方向半径（一般为短轴）
 * @param arc       计算方向与x轴正方向的夹角（弧度） 逆时针为正（等于纬度）
 * @param elev      切点的高程值
 * @param out_arc   校正后的维度值
 * @param out_rad   校正后高程点的球心半径
 * @param x_arc     x轴正方向绕原点逆时针旋转的角度 （弧度），默认为0
 */
void gctl::ellipse_plus_elevation_2d(double x_len, double y_len, double arc, double elev,
	double &out_arc, double &out_rad, double x_arc)
{
	if (fabs(x_len - y_len) < 1e-8) // 就是个圆 直接加
	{
		out_arc = arc;
		out_rad = 0.5*(x_len + y_len) + elev;
		return;
	}
	
	if (fabs(arc) < 1e-8) // 弧度太小 直接加和
	{
		out_arc = arc;
		out_rad = x_len + elev;
		return;
	}

	if (fabs(fabs(arc) - 0.5*GCTL_Pi) < 1e-8) // 到了弧顶 直接加和
	{
		out_arc = arc;
		out_rad = y_len + elev;
		return;
	}
	
	double ellip_rad = ellipse_radius_2d(x_len, y_len, arc, x_arc);
	double alpha = atan(x_len*x_len*sin(arc)/(y_len*y_len*cos(arc)));
	double sin_alpha = sin(alpha);
	double lx = ellip_rad * sin(alpha - arc)/sin_alpha;
	double lr = ellip_rad * sin(arc)/sin_alpha + elev;
	out_rad = sqrt(lx*lx + lr*lr - 2.0*lx*lr*cos(GCTL_Pi - alpha));
	out_arc = acos((lx*lx + out_rad*out_rad - lr*lr)/(2.0*out_rad*lx));
	if (arc < 0.0) out_arc *= -1.0;

	return;
}

/**
 * @brief      椭球或者球在不同球面位置的半径
 *
 * @param[in]  x_len  x方向半径
 * @param[in]  y_len  y方向半径
 * @param[in]  z_len  z方向半径
 * @param[in]  phi    计算方向与x轴正方向的夹角（弧度） 逆时针为正
 * @param[in]  theta  计算方向与z轴正方向的夹角（弧度） 逆时针为正
 *
 * @return     半径值
 */
double gctl::ellipsoid_radius(double x_len, double y_len, double z_len, double phi, double theta)
{
	return x_len*y_len*z_len/sqrt(pow(y_len*z_len*sin(theta)*cos(phi),2) + 
		pow(x_len*z_len*sin(theta)*sin(phi),2) + pow(x_len*y_len*cos(theta),2));
}

//一维线性插值
double gctl::line_interpolate(double x1,double x2,double y1,double y2,double x)
{
	if (x1 == x2) return 0.5*(y1 + y2);
	else return (x - x1)*(y2 -y1)/(x2 - x1) + y1;
}

double gctl::line_interpolate(double xmin, double xmax, const array<double> &y_arr, double x)
{
	int ynum = y_arr.size();
	double dx = (xmax - xmin)/(ynum - 1);
	int ix = floor((x - xmin)/dx);

	return line_interpolate(xmin + ix*dx, xmin + (ix+1)*dx, y_arr[ix], y_arr[ix+1], x);
}

/*
lat
|
|
h21----h22
|       |
|       |
h11----h12----> lon
*/
// 球面双线性插值函数 单位为弧度 注意纬度为余纬度
double gctl::sph_linear_interpolate(double CoLat1, double CoLat2, double Lon1, double Lon2, 
	double CoLat0, double Lon0, double h11, double h12, double h21, double h22)
{
	double Delta=(Lon2-Lon1)*(cos(CoLat2)-cos(CoLat1));
	double A=(Lon1*(h12-h22)+Lon2*(h21-h11))/Delta;
	double B=(cos(CoLat1)*(h21-h22)+cos(CoLat2)*(h12-h11))/Delta;
	double C=(h11+h22-h21-h12)/Delta;
	double D=(Lon2*cos(CoLat2)*h11-Lon2*cos(CoLat1)*h21+Lon1*cos(CoLat1)*h22-Lon1*cos(CoLat2)*h12)/Delta;
	double h0=A*cos(CoLat0)+B*Lon0+C*Lon0*cos(CoLat0)+D;
	return h0;
}

/*
lat
|
|
h21----h22
|       |
|       |
h11----h12----> lon
*/
// 球面双线性插值函数 以度为单位的版本 注意纬度为余纬度
double gctl::sph_linear_interpolate_deg(double CoLat1,double CoLat2,double Lon1,double Lon2,
	double CoLat0,double Lon0,double h11,double h12,double h21,double h22)
{
	double Delta=(Lon2-Lon1)*(cosd(CoLat2)-cosd(CoLat1));
	double A=(Lon1*(h12-h22)+Lon2*(h21-h11))/Delta;
	double B=(cosd(CoLat1)*(h21-h22)+cosd(CoLat2)*(h12-h11))/Delta;
	double C=(h11+h22-h21-h12)/Delta;
	double D=(Lon2*cosd(CoLat2)*h11-Lon2*cosd(CoLat1)*h21+Lon1*cosd(CoLat1)*h22-Lon1*cosd(CoLat2)*h12)/Delta;
	double h0=A*cosd(CoLat0)+B*Lon0+C*Lon0*cosd(CoLat0)+D;
	return h0;
}

//规则网络插值 长方形内数据插值 距离平方反比
/*长方体示意图*/
// y
// |
// |
// 3------------2
// |            |
// |            |
// |            |
// 0------------1--->x
// 左下角坐标x0 y0
// 块体尺寸dx dy
// 插值点坐标x y
// 四个角点值
double gctl::rect_interpolate(double x0,double y0,double dx,double dy,double x,double y,
					double d0,double d1,double d2,double d3)
{
	double res = 0;
	double total_dist = 0;
	double dist[4] = {0,0,0,0};
	double val[4];
	val[0] = d0; val[1] = d1; val[2] = d2; val[3] = d3;
	dist[0] = 1.0/(1e-30+(x-x0)*(x-x0)+(y-y0)*(y-y0));
	dist[1] = 1.0/(1e-30+(x-dx-x0)*(x-dx-x0)+(y-y0)*(y-y0));
	dist[2] = 1.0/(1e-30+(x-dx-x0)*(x-dx-x0)+(y-dy-y0)*(y-dy-y0));
	dist[3] = 1.0/(1e-30+(x-x0)*(x-x0)+(y-dy-y0)*(y-dy-y0));
	for (int i = 0; i < 4; i++)
	{
		total_dist += dist[i];
	}
	for (int i = 0; i < 4; i++)
	{
		res += val[i]*dist[i]/total_dist;
	}
	return res;
}

//规则网络插值 六面体内数据插值 距离平方反比
/*六面体示意图*/
//       y
//      /
//     /
//    3------------2
//   /|           /|
//  / |          / |
// 0------------1------> x
// |  7         |  6
// | /          | /
// |/           |/
// 4------------5
// |
// |
// z
// 左上角坐标x0 y0 z0
// 块体尺寸dx dy dz
// 插值点坐标x y z
// 八个角点值
double gctl::cube_interpolate(double x0,double y0,double z0,double dx,double dy,double dz,double x,double y,double z,
	double d0,double d1,double d2,double d3,double d4,double d5,double d6,double d7)
{
	double res = 0.0;
	double total_dist = 0;
	double dist[8] = {0,0,0,0,0,0,0,0};
	double val[8];
	val[0] = d0; val[1] = d1; val[2] = d2; val[3] = d3;
	val[4] = d4; val[5] = d5; val[6] = d6; val[7] = d7;
	//计算八个角点与待插值点的距离信息
	dist[0] = 1.0/(1e-30+(x-x0)*(x-x0)+(y-y0)*(y-y0)+(z-z0)*(z-z0));
	dist[1] = 1.0/(1e-30+(x-dx-x0)*(x-dx-x0)+(y-y0)*(y-y0)+(z-z0)*(z-z0));
	dist[2] = 1.0/(1e-30+(x-dx-x0)*(x-dx-x0)+(y-dy-y0)*(y-dy-y0)+(z-z0)*(z-z0));
	dist[3] = 1.0/(1e-30+(x-x0)*(x-x0)+(y-dy-y0)*(y-dy-y0)+(z-z0)*(z-z0));
	dist[4] = 1.0/(1e-30+(x-x0)*(x-x0)+(y-y0)*(y-y0)+(z-dz-z0)*(z-dz-z0));
	dist[5] = 1.0/(1e-30+(x-dx-x0)*(x-dx-x0)+(y-y0)*(y-y0)+(z-dz-z0)*(z-dz-z0));
	dist[6] = 1.0/(1e-30+(x-dx-x0)*(x-dx-x0)+(y-dy-y0)*(y-dy-y0)+(z-dz-z0)*(z-dz-z0));
	dist[7] = 1.0/(1e-30+(x-x0)*(x-x0)+(y-dy-y0)*(y-dy-y0)+(z-dz-z0)*(z-dz-z0));
	for (int i = 0; i < 8; i++)
	{
		total_dist += dist[i];
	}
	for (int i = 0; i < 8; i++)
	{
		res += val[i]*dist[i]/total_dist;
	}
	return res;
}

//球坐标下规则网络插值 六面体内数据插值
/*六面体示意图*/
//       lat
//      /
//     /
//    7------------6
//   /|           /|
//  / |          / |
// 4------------5------> lon
// |  3         |  2
// | /          | /
// |/           |/
// 0------------1
// |
// |
// rad
// 左下角坐标lon0 lat0 rad0
// 块体尺寸dlon dlat drad
// 插值点坐标lon lat rad
// 八个角点值
double gctl::cube_interpolate_sph(double lon0, double lat0, double rad0, double dlon, double dlat, double drad, 
	double lon, double lat, double rad, double d0, double d1, double d2, double d3, double d4, double d5, 
	double d6, double d7)
{
	double d04 = line_interpolate(rad0, rad0+drad, d0, d4, rad);
	double d15 = line_interpolate(rad0, rad0+drad, d1, d5, rad);
	double d26 = line_interpolate(rad0, rad0+drad, d2, d6, rad);
	double d37 = line_interpolate(rad0, rad0+drad, d3, d7, rad);
	return sph_linear_interpolate_deg(90.0-lat0, 90.0-lat0-dlat, lon0, lon0+dlon, 90.0-lat, lon, 
		d04, d15, d37, d26);
}

/**
 * @brief      按距离反比加权计算均值
 *
 * @param      dis_vec  距离向量
 * @param      val_vec  数值向量
 * @param[in]  order    距离加权的阶次 默认为1
 *
 * @return     加权平均值
 */
double gctl::dist_inverse_weight(std::vector<double> *dis_vec, std::vector<double> *val_vec, int order)
{
	if (dis_vec->size() != val_vec->size())
		throw runtime_error("The arrays have different sizes. Thrown by gctl::dist_inverse_weight(...)");

	double total_dist = 0;
	for (int i = 0; i < dis_vec->size(); i++)
	{
		dis_vec->at(i) = 1.0/(GCTL_ZERO + pow(dis_vec->at(i),order));
		total_dist += dis_vec->at(i);
	}

	double ret = 0.0;
	for (int i = 0; i < dis_vec->size(); i++)
	{
		ret += val_vec->at(i)*dis_vec->at(i)/total_dist;
	}
	return ret;
}

/**
 * @brief      使用cosine函数对一维数组进行外插
 *
 * @param      in_arr     输入数组
 * @param[in]  in_num     数组长度
 * @param      out_arr    输出数组
 * @param[in]  out_num    数组长度
 * @param[in]  end_value  外插后的端点值，若为NAN则函数会计算原数组端点值的均值作为新的端点值
 */
void gctl::cosine_extrapolate_1d(double *in_arr, int in_num, double *out_arr, int out_num, double end_value)
{
	if (in_arr == nullptr || out_arr == nullptr)
		throw domain_error("Invalid pointer. Thrown by gctl::cosine_extrapolate_1d(...)");

	if (in_num >= out_num)
		throw invalid_argument("Incompatible output array size. Thrown by gctl::cosine_extrapolate_1d(...)");

	if (std::isnan(end_value))
	{
		end_value = 0.5*(in_arr[0] + in_arr[in_num-1]);
	}

	// 确定左右侧外插值数量
	int l_num = floor((out_num - in_num)/2.0);
	int r_num = ceil((out_num - in_num)/2.0);

	for (int i = 0; i < l_num; i++)
	{
		out_arr[i] = end_value + (in_arr[0]-end_value)*cos(-0.5*GCTL_Pi*(i-l_num)/l_num);
	}
	for (int i = 0; i < in_num; i++)
	{
		out_arr[l_num+i] = in_arr[i];
	}
	for (int i = 0; i < r_num; i++)
	{
		out_arr[l_num+in_num+i] = end_value + (in_arr[in_num-1]-end_value)*cos(0.5*GCTL_Pi*(i+1)/r_num);
	}
	return;
}

/**
 * @brief      使用cosine函数对一维数组进行外插
 *
 * @param[in]  in_arr   输入数组
 * @param      out_arr  输出数组
 * @param[in]  end_val  外插后的端点值，若输入值为NAN函数会计算原数组端点值的均值作为新的端点值
 * @param[in]  out_len  外插后的数组长度，若输入值为-1函数会计算最小的2的次方值作为输出的数组长度
 */
void gctl::cosine_extrapolate_1d(const array<double> &in_arr, array<double> &out_arr, double end_val, int out_len)
{
	if (in_arr.empty())
		throw domain_error("The input array is empty. From gctl::cosine_extrapolate_1d(...)");

	int ex_len;
	if (out_len < 0)
	{
		ex_len = pow(2,ceil(log(in_arr.size())/log(2))+1);
	}
	else ex_len = out_len;

	out_arr.resize(ex_len, 0.0);

	cosine_extrapolate_1d(in_arr.get(), in_arr.size(), out_arr.get(), ex_len, end_val);
	return;
}

/**
 * @brief      使用cosine函数对二维数组进行外插
 *
 * @param[in]  in_arr    输入数组
 * @param      out_arr   输出数组
 * @param      ori_row   原数据在扩边后的起始行号
 * @param      ori_col   原数据在扩边后的起始列号
 * @param[in]  end_valx  x方向外插后的端点值，若输入值为NAN函数会计算原数组端点值的均值作为新的端点值
 * @param[in]  end_valy  y方向外插后的端点值，若输入值为NAN函数会计算原数组端点值的均值作为新的端点值
 * @param[in]  out_lenx  x方向外插后的数组长度，若输入值小于0函数会计算最小的2的次方值作为输出的数组长度
 * @param[in]  out_leny  y方向外插后的数组长度，若输入值小于0函数会计算最小的2的次方值作为输出的数组长度
 */
void gctl::cosine_extrapolate_2d(const _2d_matrix &in_arr, _2d_matrix &out_arr, 
	int &ori_row, int &ori_col, double end_valx, double end_valy, int out_lenx, int out_leny)
{
	if (in_arr.empty())
		throw domain_error("The input array is empty. From gctl::cosine_extrapolate_2d(...)");

	int M_ex, N_ex;
	int M = in_arr.row_size();
	int N = in_arr.col_size();
	if (out_lenx < 0)
	{
		N_ex = pow(2,ceil(log(N)/log(2))+1);
	}
	else N_ex = out_lenx;

	if (out_leny < 0)
	{
		M_ex = pow(2,ceil(log(M)/log(2))+1);
	}
	else M_ex = out_leny;

	if (M >= M_ex || N >= N_ex)
		throw logic_error("Invalid input or output array size. From gctl::cosine_extrapolate_2d(...)");

	out_arr.resize(M_ex, N_ex, 0.0);

	gctl::array<double> endmean(N);
	gctl::array<double> endmean2(M_ex);

	int M_ex_up = floor((M_ex - M)/2.0);
	int M_ex_down = ceil((M_ex - M)/2.0);
	int N_ex_left = floor((N_ex - N)/2.0);
	int N_ex_right = ceil((N_ex - N)/2.0);
	ori_row = M_ex_down;
	ori_col = N_ex_left;

	if (std::isnan(end_valy))
	{
		for (int i = 0; i < N; i++)
			endmean[i] = (in_arr[0][i]+in_arr[M-1][i])/2.0;
	}
	else
	{
		for (int i = 0; i < N; i++)
			endmean[i] = end_valy;
	}

	for (int i = 0; i < N; i++)
	{
		for (int k = 0; k < M_ex_down; k++)
		{
			out_arr[k][N_ex_left+i] = endmean[i] + (in_arr[0][i]-endmean[i])*cos(-0.5*GCTL_Pi*(k-M_ex_down)/M_ex_down);
		}
		for (int k = 0; k < M; k++)
		{
			out_arr[M_ex_down+k][N_ex_left+i] = in_arr[k][i];
		}
		for (int k = 0; k < M_ex_up; k++)
		{
			out_arr[M_ex_down+M+k][N_ex_left+i] = endmean[i] + (in_arr[M-1][i]-endmean[i])*cos(0.5*GCTL_Pi*(k+1)/M_ex_up); 
		}
	}

	if (std::isnan(end_valx))
	{
		for (int i = 0; i < M_ex; i++)
			endmean2[i] = (out_arr[i][N_ex_left]+out_arr[i][N_ex_left+N-1])/2.0;
	}
	else
	{
		for (int i = 0; i < M_ex; i++)
			endmean2[i] = end_valx;
	}

	for (int i = 0; i < M_ex; i++)
	{
		for (int k = 0; k < N_ex_left; k++)
		{
			out_arr[i][k] = endmean2[i] + (out_arr[i][N_ex_left]-endmean2[i])*cos(-0.5*GCTL_Pi*(k-N_ex_left)/N_ex_left);
		}
		for (int k = 0; k < N_ex_right; k++)
		{
			out_arr[i][N_ex_left+N+k] = endmean2[i] + (out_arr[i][N_ex_left+N-1]-endmean2[i])*cos(0.5*GCTL_Pi*(k+1)/N_ex_right);
		}
	}
	return;
}

/**
 * @brief      使用cosine函数对二维数组进行外插
 * 
 * 按一维数组排列（列优先，按行排列）
 *
 * @param[in]  in_arr    输入数组
 * @param      out_arr   输出数组
 * @param[in]  in_row    输入数组的行数
 * @param[in]  in_col    输入数组的列数
 * @param      out_row   输出数组的行数（如果输入值为负则函数会计算最小的2的次方值作为输出的行数，否则即按照输入值进行扩边）
 * @param      out_col   输出数组的列数（如果输入值为负则函数会计算最小的2的次方值作为输出的行数，否则即按照输入值进行扩边）
 * @param      ori_row   原数据在扩边后的起始行号
 * @param      ori_col   原数据在扩边后的起始列号
 * @param[in]  end_valx  x方向外插后的端点值，若输入值为NAN函数会计算原数组端点值的均值作为新的端点值
 * @param[in]  end_valy  y方向外插后的端点值，若输入值为NAN函数会计算原数组端点值的均值作为新的端点值
 */
void gctl::cosine_extrapolate_2d(const array<double> &in_arr, array<double> &out_arr, int in_row, int in_col, 
	int &out_row, int &out_col, int &ori_row, int &ori_col, double end_valx, double end_valy)
{
	if (in_arr.empty())
		throw runtime_error("The input array is empty. Thrown by gctl::cosine_extrapolate_2d(...)");

	if (in_row*in_col != in_arr.size())
		throw invalid_argument("Incompatible array sizes. Thrown by gctl::cosine_extrapolate_2d(...)");

	int M = in_row;
	int N = in_col;

	if (out_col < 0) out_col = pow(2,ceil(log(N)/log(2))+1);
	else if (out_col <= in_col)
	{
		throw invalid_argument("Incompatible output column size. Thrown by gctl::cosine_extrapolate_2d(...)");
	}
	int N_ex = out_col;

	if (out_row < 0) out_row = pow(2,ceil(log(M)/log(2))+1);
	else if (out_row <= in_row)
	{
		throw invalid_argument("Incompatible output row size. Thrown by gctl::cosine_extrapolate_2d(...)");
	}
	int M_ex = out_row;

	out_arr.resize(M_ex*N_ex, 0.0);

	gctl::array<double> endmean(N);
	gctl::array<double> endmean2(M_ex);

	int M_ex_up = floor((M_ex - M)/2.0);
	int M_ex_down = ceil((M_ex - M)/2.0);
	int N_ex_left = floor((N_ex - N)/2.0);
	int N_ex_right = ceil((N_ex - N)/2.0);
	ori_row = M_ex_down;
	ori_col = N_ex_left;

	if (std::isnan(end_valy))
	{
		for (int i = 0; i < N; i++)
			endmean[i] = (in_arr[i]+in_arr[(M-1)*N+i])/2.0;
	}
	else
	{
		for (int i = 0; i < N; i++)
			endmean[i] = end_valy;
	}

	for (int i = 0; i < N; i++)
	{
		for (int k = 0; k < M_ex_down; k++)
		{
			out_arr[k*N_ex+N_ex_left+i] = endmean[i] + (in_arr[i]-endmean[i])*cos(-0.5*GCTL_Pi*(k-M_ex_down)/M_ex_down);
		}
		for (int k = 0; k < M; k++)
		{
			out_arr[(M_ex_down+k)*N_ex+N_ex_left+i] = in_arr[k*N+i];
		}
		for (int k = 0; k < M_ex_up; k++)
		{
			out_arr[(M_ex_down+M+k)*N_ex+N_ex_left+i] = endmean[i] + (in_arr[(M-1)*N+i]-endmean[i])*cos(0.5*GCTL_Pi*(k+1)/M_ex_up); 
		}
	}

	if (std::isnan(end_valx))
	{
		for (int i = 0; i < M_ex; i++)
			endmean2[i] = (out_arr[i*N_ex+N_ex_left]+out_arr[i*N_ex+N_ex_left+N-1])/2.0;
	}
	else
	{
		for (int i = 0; i < M_ex; i++)
			endmean2[i] = end_valx;
	}

	for (int i = 0; i < M_ex; i++)
	{
		for (int k = 0; k < N_ex_left; k++)
		{
			out_arr[i*N_ex+k] = endmean2[i] + (out_arr[i*N_ex+N_ex_left]-endmean2[i])*cos(-0.5*GCTL_Pi*(k-N_ex_left)/N_ex_left);
		}
		for (int k = 0; k < N_ex_right; k++)
		{
			out_arr[i*N_ex+N_ex_left+N+k] = endmean2[i] + (out_arr[i*N_ex+N_ex_left+N-1]-endmean2[i])*cos(0.5*GCTL_Pi*(k+1)/N_ex_right);
		}
	}
	return;
}

/**
 * @brief      使用零值对二维数组进行外插
 * 
 * 按一维数组排列（列优先，按行排列）
 *
 * @param[in]  in_arr    输入数组
 * @param      out_arr   输出数组
 * @param[in]  in_row    输入数组的行数
 * @param[in]  in_col    输入数组的列数
 * @param      out_row   输出数组的行数（如果输入值为负则函数会计算最小的2的次方值作为输出的行数，否则即按照输入值进行扩边）
 * @param      out_col   输出数组的列数（如果输入值为负则函数会计算最小的2的次方值作为输出的行数，否则即按照输入值进行扩边）
 * @param      ori_row   原数据在扩边后的起始行号
 * @param      ori_col   原数据在扩边后的起始列号
 */
void gctl::zeros_extrapolate_2d(const array<double> &in_arr, array<double> &out_arr, int in_row, int in_col, 
	int &out_row, int &out_col, int &ori_row, int &ori_col)
{
	if (in_arr.empty())
		throw runtime_error("The input array is empty. Thrown by gctl::zeros_extrapolate_2d(...)");

	if (in_row*in_col != in_arr.size())
		throw invalid_argument("Incompatible array sizes. Thrown by gctl::zeros_extrapolate_2d(...)");

	int M = in_row;
	int N = in_col;

	if (out_col < 0) out_col = pow(2,ceil(log(N)/log(2))+1);
	else if (out_col <= in_col)
	{
		throw invalid_argument("Incompatible output column size. Thrown by gctl::zeros_extrapolate_2d(...)");
	}
	int N_ex = out_col;

	if (out_row < 0) out_row = pow(2,ceil(log(M)/log(2))+1);
	else if (out_row <= in_row)
	{
		throw invalid_argument("Incompatible output row size. Thrown by gctl::zeros_extrapolate_2d(...)");
	}
	int M_ex = out_row;

	out_arr.resize(M_ex*N_ex, 0.0);

	int M_ex_down = ceil((M_ex - M)/2.0);
	int N_ex_left = floor((N_ex - N)/2.0);
	ori_row = M_ex_down;
	ori_col = N_ex_left;

	for (int i = 0; i < M; i++)
	{
		for (int j = 0; j < N; j++)
		{
			out_arr[(M_ex_down+i)*N_ex+N_ex_left+j] = in_arr[i*N+j];
		}
	}
	return;
}

//我们使用二分法查找位置
//如果查找成功则返回0，否则返回-1
int gctl::find_index(const double *in_array, int array_size, double in_val, int &index)
{
	if (array_size <= 0)
	{
		index = -1;
		return -1;
	}
	else if (array_size == 1)
	{
		index = -1;
		return -1;
	}
	else if (in_val < in_array[0] || in_val > in_array[array_size-1])
	{
		index = -1;
		return -1;
	}
	else if (array_size == 2)
	{
		index = 0;
		return 0;
	}
	else
	{
		int low_range = 0;
		int high_range = array_size - 1;
		int test_index;
		bool found = false;
		while (high_range - low_range >= 1)
		{
			test_index = floor(0.5*(low_range + high_range));
			if (in_val >= in_array[test_index] && in_val <= in_array[test_index+1])
			{
				index = test_index;
				found = true;
				break;
			}
			else if (in_val < in_array[test_index])
			{
				high_range = test_index;
			}
			else if (in_val > in_array[test_index+1])
			{
				low_range = test_index+1;
			}
		}
		if (found) return 0;
		else return -1;
	}
}

//查找一个数在已排序的数组中的位置，即找到包含该数的最小区间 重载
int gctl::find_index(array<double> *in_array, double in_val, int &index)
{
	return find_index(in_array->get(), in_array->size(), in_val, index);
}

void gctl::fractal_model_1d(array<double> &out_arr, int out_size, double l_val, 
	double r_val, double maxi_range, double smoothness)
{
	if (out_size <= 0)
		throw invalid_argument("Negative output size. Thrown by gctl::fractal_model_1d(...)");
	if (maxi_range <= 0)
		throw invalid_argument("Negative maximal range. Thrown by gctl::fractal_model_1d(...)");
	if (smoothness <= 0)
		throw invalid_argument("Negative smoothness. Thrown by gctl::fractal_model_1d(...)");

	out_arr.resize(out_size);
	int bigger_num = (int) pow(2, ceil(log(out_arr.size()-1)/log(2))) + 1;
	array<double> tmp_arr(bigger_num); // 计算的长度必须为2的次方数

	int step_size = (int) (bigger_num-1)/2;

	tmp_arr[0] = l_val;
	tmp_arr[bigger_num-1] = r_val;

	srand(time(0));

	while (step_size >= 1)
	{
		for (int i = step_size; i < bigger_num; i += 2*step_size)
		{
			tmp_arr[i] = random(-1.0*maxi_range, maxi_range) + 
				0.5*(tmp_arr[i-step_size] + tmp_arr[i+step_size]);
		}
		maxi_range = pow(2.0, -1.0*smoothness)*maxi_range;
		step_size /= 2;
	}

	for (int i = 0; i < out_arr.size(); i++)
	{
		out_arr[i] = tmp_arr[i];
	}
	return;
}

void gctl::fractal_model_2d(_2d_matrix &out_arr, int r_size, int c_size, double dl_val, 
	double dr_val, double ul_val, double ur_val, double maxi_range, double smoothness)
{
	if (r_size <= 0 || c_size <= 0)
		throw invalid_argument("Negative output size. Thrown by gctl::fractal_model_2d(...)");
	if (maxi_range <= 0)
		throw invalid_argument("Negative maximal range. Thrown by gctl::fractal_model_2d(...)");
	if (smoothness <= 0)
		throw invalid_argument("Negative smoothness. Thrown by gctl::fractal_model_2d(...)");

	int i, j, m, n, R, jmax;
	double random_d; 

	out_arr.resize(r_size, c_size);
	int xnum = out_arr.col_size();
	int ynum = out_arr.row_size();

	int order_x = ceil(log(xnum-1)/log(2));
	int order_y = ceil(log(ynum-1)/log(2));
	int imax = GCTL_MAX(order_x,order_y);
	int ntotal = (int) pow(2.0, imax) + 1; //总数据点数为2的imax次方加1

	_2d_matrix topo(ntotal, ntotal, 0.0);
	for (i=0; i<ntotal; i++)//设定地形数据初始值，角点数据必须给出
	{
		for (j=0; j<ntotal; j++)
		{
			if (i == 0 && j == 0)
			{
				topo[i][j] = ul_val; //角点初始值；
			} 
			else if (i == ntotal-1 && j==0)
			{
				topo[i][j] = dl_val; //角点初始值；
			} 
			else if (i==0 && j==ntotal-1)
			{
				topo[i][j] = ur_val; //角点初始值；
			} 
			else if (i==ntotal-1 && j==ntotal-1)
			{
				topo[i][j] = dr_val; //角点初始值；
			} 
			else
			{
				topo[i][j] = GCTL_BDL_MAX;
			}
		}
	}

	srand(time(0));

	for (i = 1; i <= imax; i++)//开始迭代，生成正方形区域随机地形
	{
		R = int(double(ntotal-1)/pow(2.0,i));

		jmax=int(pow(4.0,i-1));

		for (j=1; j<=jmax; j++)
		{
			random_d = random(-1.0*maxi_range, maxi_range);

			m=2*R*(j-(ceil((double)j/pow(2.0,i-1))-1) * pow(2.0,i-1))-R;
			n=2*R*(ceil((double)j/pow(2.0,i-1)))-R;
			topo[m][n]=(topo[m-R][n-R]+topo[m+R][n-R]+topo[m-R][n+R]+topo[m+R][n+R])/4+random_d;
		}

		for (j=1; j<=jmax; j++)
		{
			m=2*R*(j-(ceil((double)j/pow(2.0,i-1))-1)* pow(2.0,i-1))-R;
			n=2*R*(ceil((double)j/pow(2.0,i-1)))-R;

			if (topo[m][n-R] == GCTL_BDL_MAX)
			{
				random_d = random(-1.0*maxi_range, maxi_range);

				if ((n-R)!=0)
				{
					topo[m][n-R]=(topo[m][n-2*R]+topo[m-R][n-R]+topo[m+R][n-R]+topo[m][n])/4+random_d;
				} 
				else
				{
					topo[m][n-R]=(topo[m-R][n-R]+topo[m+R][n-R]+topo[m][n])/3+random_d;
				}
			}

			if (topo[m-R][n] == GCTL_BDL_MAX)
			{
				random_d = random(-1.0*maxi_range, maxi_range);

				if ((m-R)!=0)
				{
					topo[m-R][n]=(topo[m-R][n-R]+topo[m-2*R][n]+topo[m][n]+topo[m-R][n+R])/4+random_d;
				} 
				else
				{
					topo[m-R][n]=(topo[m-R][n-R]+topo[m][n]+topo[m-R][n+R])/3+random_d;
				}
			}

			if (topo[m+R][n] == GCTL_BDL_MAX)
			{
				random_d = random(-1.0*maxi_range, maxi_range);

				if ((m+R)!=(ntotal-1))
				{
					topo[m+R][n]=(topo[m+R][n-R]+topo[m][n]+topo[m+2*R][n]+topo[m+R][n+R])/4+random_d;
				} 
				else
				{
					topo[m+R][n]=(topo[m+R][n-R]+topo[m][n]+topo[m+R][n+R])/3+random_d;
				}
			}

			if (topo[m][n+R] == GCTL_BDL_MAX)
			{
				random_d = random(-1.0*maxi_range, maxi_range);

				if ((n+R)!=(ntotal-1))
				{
					topo[m][n+R]=(topo[m][n]+topo[m-R][n+R]+topo[m+R][n+R]+topo[m][n+2*R])/4+random_d;
				} 
				else
				{
					topo[m][n+R]=(topo[m][n]+topo[m-R][n+R]+topo[m+R][n+R])/3+random_d;
				}
			}
		}

		maxi_range = pow(2.0, -1.0*smoothness)*maxi_range;
	}

	for (int j = 0; j < ynum; j++)//按预设区域剪裁数组
	{
		for (int i=0; i < xnum; i++)
		{
			out_arr[i][j] = topo[i][j];
		}
	}

	return;
}

// 使用牛顿迭代法计算一个矩阵的近似逆
double gctl::newton_inverse(const _2d_matrix &in_mat, _2d_matrix &inverse_mat, 
	double epsilon, int iter_times, bool initiate)
{
	if (in_mat.empty())
		throw runtime_error("The input matrix is empty. Thrown by gctl::newton_inverse(...)");

	if (in_mat.row_size() != in_mat.col_size())
		throw logic_error("The input matrix is square. Thrown by gctl::newton_inverse(...)");

	if (iter_times < 0)
		throw invalid_argument("Invalid iteration times. Thrown by gctl::newton_inverse(...)");

	int m_size = in_mat.row_size();

	if (initiate)
	{
		// 初始化逆矩阵 使用输入矩阵的对角元素构建初始矩阵
		if (!inverse_mat.empty()) inverse_mat.clear();
		inverse_mat.resize(m_size, m_size, 0.0);
		for (int i = 0; i < m_size; i++)
		{
			inverse_mat[i][i] = 1.0/in_mat[i][i];
		}
	}

	if (inverse_mat.row_size() != m_size || inverse_mat.col_size() != m_size)
		throw logic_error("Invalid output matrix size. From gctl::newton_inverse(...)");

	// 迭代的收敛条件 || E - in_mat*inverse_mat ||_inf < 1
	// 计算矩阵的无穷大范数
	double maxi_mod = 0.0, mod, ele;
	for (int i = 0; i < m_size; i++)
	{
		mod = 0.0;
		for (int j = 0; j < m_size; j++)
		{
			ele = 0.0;
			for (int k = 0; k < m_size; k++)
			{
				ele += in_mat[i][k] * inverse_mat[k][j];
			}

			if (i == j)
			{
				mod += GCTL_FABS(1.0 - ele);
			}
			else
			{
				mod += GCTL_FABS(ele);
			}
		}

		maxi_mod = GCTL_MAX(maxi_mod, mod);
	}

	if (maxi_mod >= 1.0)
	{
		GCTL_ShowWhatError("The iteration may not converge. From gctl::newton_inverse(...)", 
			GCTL_WARNING_ERROR, 0, 0, 0);
	}

	array<double> col_ax(m_size, 0.0); // A*X 的列向量
	_2d_matrix tmp_mat(m_size, m_size, 0.0);
	for (int t = 0; t < iter_times; t++)
	{
		if (maxi_mod <= epsilon)
			break;

		for (int i = 0; i < m_size; i++)
		{
			for (int j = 0; j < m_size; j++)
			{
				tmp_mat[i][j] = inverse_mat[i][j];
			}
		}

		for (int n = 0; n < m_size; n++)
		{
			for (int i = 0; i < m_size; i++)
			{
				for (int j = 0; j < m_size; j++)
				{
					col_ax[j] = 0.0;
					for (int k = 0; k < m_size; k++)
					{
						col_ax[j] += in_mat[j][k] * tmp_mat[k][i];
					}
					col_ax[j] *= -1.0;
				}
				col_ax[i] += 2.0;

				ele = 0.0;
				for (int j = 0; j < m_size; j++)
				{
					ele += tmp_mat[n][j] * col_ax[j];
				}

				inverse_mat[n][i] = ele;
			}
		}

		maxi_mod = 0.0;
		for (int i = 0; i < m_size; i++)
		{
			mod = 0.0;
			for (int j = 0; j < m_size; j++)
			{
				ele = 0.0;
				for (int k = 0; k < m_size; k++)
				{
					ele += in_mat[i][k] * inverse_mat[k][j];
				}

				if (i == j)
				{
					mod += GCTL_FABS(1.0 - ele);
				}
				else
				{
					mod += GCTL_FABS(ele);
				}
			}

			maxi_mod = GCTL_MAX(maxi_mod, mod);
		}
	}

	return maxi_mod;
}

// 使用牛顿迭代法计算一个矩阵的近似逆
double gctl::newton_inverse(const spmat<double> &in_mat, _2d_matrix &inverse_mat, 
	double epsilon, int iter_times, bool initiate)
{
	if (in_mat.empty())
		throw runtime_error("The input matrix is empty. Thrown by gctl::newton_inverse(...)");

	if (in_mat.row_size() != in_mat.col_size())
		throw logic_error("The input matrix is square. Thrown by gctl::newton_inverse(...)");

	if (iter_times < 0)
		throw invalid_argument("Invalid iteration times. Thrown by gctl::newton_inverse(...)");

	int m_size = in_mat.row_size();

	if (initiate)
	{
		// 初始化逆矩阵 使用输入矩阵的对角元素构建初始矩阵
		if (!inverse_mat.empty()) inverse_mat.clear();
		inverse_mat.resize(m_size, m_size, 0.0);
		for (int i = 0; i < m_size; i++)
		{
			inverse_mat[i][i] = 1.0/in_mat.at(i, i);
		}
	}

	if (inverse_mat.row_size() != m_size || inverse_mat.col_size() != m_size)
		throw logic_error("Invalid output matrix size. From gctl::newton_inverse(...)");

	// 迭代的收敛条件 || E - in_mat*inverse_mat ||_inf < 1
	// 计算矩阵的无穷大范数
	double maxi_mod = 0.0, mod, ele;
	array<double> tmp_arr(m_size, 0.0); // A*X 的列向量
	for (int i = 0; i < m_size; i++)
	{
		mod = 0.0;
		for (int j = 0; j < m_size; j++)
		{
			for (int k = 0; k < m_size; k++)
			{
				tmp_arr[k] = inverse_mat[k][j];
			}

			ele = in_mat.multiply_vector(tmp_arr, i);

			if (i == j)
			{
				mod += GCTL_FABS(1.0 - ele);
			}
			else
			{
				mod += GCTL_FABS(ele);
			}
		}

		maxi_mod = GCTL_MAX(maxi_mod, mod);
	}

	if (maxi_mod >= 1.0)
	{
		GCTL_ShowWhatError("The iteration may not converge. From gctl::newton_inverse(...)", 
			GCTL_WARNING_ERROR, 0, 0, 0);
	}

	array<double> col_ax(m_size, 0.0);
	_2d_matrix tmp_mat(m_size, m_size, 0.0);
	for (int t = 0; t < iter_times; t++)
	{
		if (maxi_mod <= epsilon)
			break;
		//else std::cout << "epsilon = " << maxi_mod << std::endl;

		for (int i = 0; i < m_size; i++)
		{
			for (int j = 0; j < m_size; j++)
			{
				tmp_mat[i][j] = inverse_mat[i][j];
			}
		}

		for (int n = 0; n < m_size; n++)
		{
			for (int i = 0; i < m_size; i++)
			{
				for (int j = 0; j < m_size; j++)
				{
					for (int k = 0; k < m_size; k++)
					{
						tmp_arr[k] = tmp_mat[k][i];
					}
					col_ax[j] = in_mat.multiply_vector(tmp_arr, j);
					col_ax[j] *= -1.0;
				}
				col_ax[i] += 2.0;

				ele = 0.0;
				for (int j = 0; j < m_size; j++)
				{
					ele += tmp_mat[n][j] * col_ax[j];
				}

				inverse_mat[n][i] = ele;
			}
		}

		maxi_mod = 0.0;
		for (int i = 0; i < m_size; i++)
		{
			mod = 0.0;
			for (int j = 0; j < m_size; j++)
			{
				for (int k = 0; k < m_size; k++)
				{
					tmp_arr[k] = inverse_mat[k][j];
				}

				ele = in_mat.multiply_vector(tmp_arr, i);

				if (i == j)
				{
					mod += GCTL_FABS(1.0 - ele);
				}
				else
				{
					mod += GCTL_FABS(ele);
				}
			}

			maxi_mod = GCTL_MAX(maxi_mod, mod);
		}
	}

	return maxi_mod;
}

void gctl::schmidt_orthogonal(const array<double> &a, array<double> &e, int a_s)
{
	if (a.empty())
		throw runtime_error("The input array is empty. Thrown by gctl::schmidt_orthogonal(...)");

	if (a_s <= 1) // a_s >= 2
		throw invalid_argument("vector size must be bigger than one. Thrown by gctl::schmidt_orthogonal(...)");

	int t_s = a.size();
	if (t_s%a_s != 0 || t_s <= 3) // t_s >= 4
		throw invalid_argument("incompatible total array size. Thrown by gctl::schmidt_orthogonal(...)");

	int len = t_s/a_s;
	if (len < a_s)
		throw invalid_argument("the vectors are over-determined. Thrown by gctl::schmidt_orthogonal(...)");

	e.resize(t_s, 0.0);

	double ae, ee;
	for (int i = 0; i < a_s; i++)
	{
		for (int l = 0; l < len; l++)
		{
			e[l + i*len] = a[l + i*len];
		}

		for (int m = 0; m < i; m++)
		{
			ae = ee = 0.0;
			for (int n = 0; n < len; n++)
			{
				ae += a[n + i*len] * e[n + m*len];
				ee += e[n + m*len] * e[n + m*len];
			}

			for (int n = 0; n < len; n++)
			{
				e[n + i*len] -= e[n + m*len] * ae/ee;
			}
		}
	}

	for (int i = 0; i < a_s; i++)
	{
		ee = 0.0;
		for (int l = 0; l < len; l++)
		{
			ee += e[l + i*len] * e[l + i*len];
		}
		ee = sqrt(ee);

		for (int l = 0; l < len; l++)
		{
			e[l + i*len] /= ee;
		}
	}
	return;
}

void gctl::difference_1d(const array<double> &in, array<double> &diff, double spacing, int order)
{
	if (order < 1 || order > 4)
		throw invalid_argument("The input order can only be 1 to 4. Thrown by gctl::difference_1d(...)");

	if (spacing <= 0.0)
		throw invalid_argument("The input spacing can't be negative or zero. Thrown by void gctl::difference_1d(...)");

	if (order == 1 && in.size() < 3)
		throw runtime_error("The input array size must be equal to or bigger than three for the first order derivative. Thrown by gctl::difference_1d(...)");

	if (order == 2 && in.size() < 4)
		throw runtime_error("The input array size must be equal to or bigger than four for the second order derivative. Thrown by gctl::difference_1d(...)");

	if (order == 3 && in.size() < 6)
		throw runtime_error("The input array size must be equal to or bigger than six for the third order derivative. Thrown by gctl::difference_1d(...)");

	if (order == 4 && in.size() < 7)
		throw runtime_error("The input array size must be equal to or bigger than seven for the fourth order derivative. Thrown by gctl::difference_1d(...)");

	int t_size = in.size();
	diff.resize(t_size);

	// 利用向前或向后差分计算边缘元素的导数
	if (order == 1)
	{
		diff[0] = (-3.0*in[0]+4.0*in[1]-in[2])/(2.0*spacing);
		diff[t_size-1] = (3.0*in[t_size-1]-4.0*in[t_size-2]+in[t_size-3])/(2.0*spacing);

		for (int i = 1; i < t_size-1; i++)
		{
			diff[i] = (in[i+1] - in[i-1])/(2.0*spacing);
		}
	}
	else if (order == 2)
	{
		diff[0] = (2.0*in[0]-5.0*in[1]+4.0*in[2]-in[3])/(spacing*spacing);
		diff[t_size-1] = (2.0*in[t_size-1]-5.0*in[t_size-2]+4.0*in[t_size-3]-in[t_size-4])/(spacing*spacing);

		for (int i = 1; i < t_size-1; i++)
		{
			diff[i] = (in[i-1]-2.0*in[i]+in[i+1])/(spacing*spacing);
		}
	}
	else if (order == 3)
	{
		diff[0] = (-5.0*in[0]+18.0*in[1]-24.0*in[2]+14.0*in[3]-3.0*in[4])/(2.0*spacing*spacing*spacing);
		diff[1] = (-5.0*in[1]+18.0*in[2]-24.0*in[3]+14.0*in[4]-3.0*in[5])/(2.0*spacing*spacing*spacing);
		diff[t_size-1] = (5.0*in[t_size-1]-18.0*in[t_size-2]+24.0*in[t_size-3]-14.0*in[t_size-4]+3.0*in[t_size-5])/(2.0*spacing*spacing*spacing);
		diff[t_size-2] = (5.0*in[t_size-2]-18.0*in[t_size-3]+24.0*in[t_size-4]-14.0*in[t_size-5]+3.0*in[t_size-6])/(2.0*spacing*spacing*spacing);

		for (int i = 2; i < t_size-2; i++)
		{
			diff[i] = (-1.0*in[i-2]+2.0*in[i-1]-2.0*in[i+1]+in[i+2])/(2.0*spacing*spacing*spacing);
		}
	}
	else
	{
		diff[0] = (3.0*in[0]-14.0*in[1]+26.0*in[2]-24.0*in[3]+11.0*in[4]-2.0*in[5])/(spacing*spacing*spacing*spacing);
		diff[1] = (3.0*in[1]-14.0*in[2]+26.0*in[3]-24.0*in[4]+11.0*in[5]-2.0*in[6])/(spacing*spacing*spacing*spacing);
		diff[t_size-1] = (3.0*in[t_size-1]-14.0*in[t_size-2]+26.0*in[t_size-3]-24.0*in[t_size-4]+11.0*in[t_size-5]-2.0*in[t_size-6])/(spacing*spacing*spacing*spacing);
		diff[t_size-2] = (3.0*in[t_size-2]-14.0*in[t_size-3]+26.0*in[t_size-4]-24.0*in[t_size-5]+11.0*in[t_size-6]-2.0*in[t_size-7])/(spacing*spacing*spacing*spacing);

		for (int i = 2; i < t_size-2; i++)
		{
			diff[i] = (in[i-2]-4.0*in[i-1]+6.0*in[i]-4.0*in[i+1]+in[i+2])/(spacing*spacing*spacing*spacing);
		}
	}
	return;
}

void gctl::difference_2d(const _2d_matrix &in, _2d_matrix &diff, double spacing, gradient_type_e d_type, int order)
{
	std::string err_str;
	if (order < 1 || order > 4)
		throw invalid_argument("The input order can only be 1 to 4. Thrown by void gctl::difference_2d(...)");

	if (spacing <= 0.0)
		throw invalid_argument("The input spacing can't be negative or zero. Thrown by void gctl::difference_2d(...)");

	int t_size;
	if (d_type == Dx)
	{
		t_size = in.col_size();
	}
	else if (d_type == Dy)
	{
		t_size = in.row_size();
	}
	else
	{
		throw logic_error("The calculation type must be Dx or Dy. Thrown by gctl::difference_2d(...)");
	}

	if (order == 1 && t_size < 3)
		throw runtime_error("The input array size must be equal to or bigger than 3x3 for the first order derivative. Thrown by void gctl::difference_2d(...)");

	if (order == 2 && t_size < 4)
		throw runtime_error("The input array size must be equal to or bigger than 4x4 for the second order derivative. Thrown by gctl::difference_2d(...)");

	if (order == 3 && t_size < 6)
		throw runtime_error("The input array size must be equal to or bigger than 6x6 for the third order derivative. Thrown by gctl::difference_2d(...)");

	if (order == 4 && t_size < 7)
		throw runtime_error("The input array size must be equal to or bigger than 7x7 for the fourth order derivative. Thrown by void gctl::difference_2d(...)");

	int tr_size = in.row_size(), tl_size = in.col_size();
	diff.resize(tr_size, tl_size);

	if (d_type == Dx)
	{
		if (order == 1)
		{
			for (int i = 0; i < tr_size; i++)
			{
				diff[i][0] = (-3.0*in[i][0]+4.0*in[i][1]-in[i][2])/(2.0*spacing);
				diff[i][tl_size-1] = (3.0*in[i][tl_size-1]-4.0*in[i][tl_size-2]+in[i][tl_size-3])/(2.0*spacing);

				for (int j = 1; j < tl_size-1; j++)
				{
					diff[i][j] = (in[i][j+1] - in[i][j-1])/(2.0*spacing);
				}
			}
		}
		else if (order == 2)
		{
			for (int i = 0; i < tr_size; i++)
			{
				diff[i][0] = (2.0*in[i][0]-5.0*in[i][1]+4.0*in[i][2]-in[i][3])/(spacing*spacing);
				diff[i][tl_size-1] = (2.0*in[i][tl_size-1]-5.0*in[i][tl_size-2]+4.0*in[i][tl_size-3]-in[i][tl_size-4])/(spacing*spacing);

				for (int j = 1; j < tl_size-1; j++)
				{
					diff[i][j] = (in[i][j-1]-2.0*in[i][j]+in[i][j+1])/(spacing*spacing);
				}
			}
		}
		else if (order == 3)
		{
			for (int i = 0; i < tr_size; i++)
			{
				diff[i][0] = (-5.0*in[i][0]+18.0*in[i][1]-24.0*in[i][2]+14.0*in[i][3]-3.0*in[i][4])/(2.0*spacing*spacing*spacing);
				diff[i][1] = (-5.0*in[i][1]+18.0*in[i][2]-24.0*in[i][3]+14.0*in[i][4]-3.0*in[i][5])/(2.0*spacing*spacing*spacing);
				diff[i][tl_size-1] = (5.0*in[i][tl_size-1]-18.0*in[i][tl_size-2]+24.0*in[i][tl_size-3]-14.0*in[i][tl_size-4]+3.0*in[i][tl_size-5])/(2.0*spacing*spacing*spacing);
				diff[i][tl_size-2] = (5.0*in[i][tl_size-2]-18.0*in[i][tl_size-3]+24.0*in[i][tl_size-4]-14.0*in[i][tl_size-5]+3.0*in[i][tl_size-6])/(2.0*spacing*spacing*spacing);

				for (int j = 2; j < tl_size-2; j++)
				{
					diff[i][j] = (-1.0*in[i][j-2]+2.0*in[i][j-1]-2.0*in[i][j+1]+in[i][j+2])/(2.0*spacing*spacing*spacing);
				}
			}
		}
		else
		{
			for (int i = 0; i < tr_size; i++)
			{
				diff[i][0] = (3.0*in[i][0]-14.0*in[i][1]+26.0*in[i][2]-24.0*in[i][3]+11.0*in[i][4]-2.0*in[i][5])/(spacing*spacing*spacing*spacing);
				diff[i][1] = (3.0*in[i][1]-14.0*in[i][2]+26.0*in[i][3]-24.0*in[i][4]+11.0*in[i][5]-2.0*in[i][6])/(spacing*spacing*spacing*spacing);
				diff[i][tl_size-1] = (3.0*in[i][tl_size-1]-14.0*in[i][tl_size-2]+26.0*in[i][tl_size-3]-24.0*in[i][tl_size-4]+11.0*in[i][tl_size-5]-2.0*in[i][tl_size-6])/(spacing*spacing*spacing*spacing);
				diff[i][tl_size-2] = (3.0*in[i][tl_size-2]-14.0*in[i][tl_size-3]+26.0*in[i][tl_size-4]-24.0*in[i][tl_size-5]+11.0*in[i][tl_size-6]-2.0*in[i][tl_size-7])/(spacing*spacing*spacing*spacing);

				for (int j = 2; j < tl_size-2; j++)
				{
					diff[i][j] = (in[i][j-2]-4.0*in[i][j-1]+6.0*in[i][j]-4.0*in[i][j+1]+in[i][j+2])/(spacing*spacing*spacing*spacing);
				}
			}
		}
	}
	else
	{
		if (order == 1)
		{
			for (int j = 0; j < tl_size; j++)
			{
				diff[0][j] = (-3.0*in[0][j]+4.0*in[1][j]-in[2][j])/(2.0*spacing);
				diff[tr_size-1][j] = (3.0*in[tr_size-1][j]-4.0*in[tr_size-2][j]+in[tr_size-3][j])/(2.0*spacing);

				for (int i = 1; i < tr_size-1; i++)
				{
					diff[i][j] = (in[i+1][j] - in[i-1][j])/(2.0*spacing);
				}
			}
		}
		else if (order == 2)
		{
			for (int j = 0; j < tl_size; j++)
			{
				diff[0][j] = (2.0*in[0][j]-5.0*in[1][j]+4.0*in[2][j]-in[3][j])/(spacing*spacing);
				diff[tr_size-1][j] = (2.0*in[tr_size-1][j]-5.0*in[tr_size-2][j]+4.0*in[tr_size-3][j]-in[tr_size-4][j])/(spacing*spacing);

				for (int i = 1; i < tr_size-1; i++)
				{
					diff[i][j] = (in[i-1][j]-2.0*in[i][j]+in[i+1][j])/(spacing*spacing);
				}
			}
		}
		else if (order == 3)
		{
			for (int j = 0; j < tl_size; j++)
			{
				diff[0][j] = (-5.0*in[0][j]+18.0*in[1][j]-24.0*in[2][j]+14.0*in[3][j]-3.0*in[4][j])/(2.0*spacing*spacing*spacing);
				diff[1][j] = (-5.0*in[1][j]+18.0*in[2][j]-24.0*in[3][j]+14.0*in[4][j]-3.0*in[5][j])/(2.0*spacing*spacing*spacing);
				diff[tr_size-1][j] = (5.0*in[tr_size-1][j]-18.0*in[tr_size-2][j]+24.0*in[tr_size-3][j]-14.0*in[tr_size-4][j]+3.0*in[tr_size-5][j])/(2.0*spacing*spacing*spacing);
				diff[tr_size-2][j] = (5.0*in[tr_size-2][j]-18.0*in[tr_size-3][j]+24.0*in[tr_size-4][j]-14.0*in[tr_size-5][j]+3.0*in[tr_size-6][j])/(2.0*spacing*spacing*spacing);

				for (int i = 2; i < tr_size-2; i++)
				{
					diff[i][j] = (-1.0*in[i-2][j]+2.0*in[i-1][j]-2.0*in[i+1][j]+in[i+2][j])/(2.0*spacing*spacing*spacing);
				}
			}
		}
		else
		{
			for (int j = 0; j < tl_size; j++)
			{
				diff[0][j] = (3.0*in[0][j]-14.0*in[1][j]+26.0*in[2][j]-24.0*in[3][j]+11.0*in[4][j]-2.0*in[5][j])/(spacing*spacing*spacing*spacing);
				diff[1][j] = (3.0*in[1][j]-14.0*in[2][j]+26.0*in[3][j]-24.0*in[4][j]+11.0*in[5][j]-2.0*in[6][j])/(spacing*spacing*spacing*spacing);
				diff[tr_size-1][j] = (3.0*in[tr_size-1][j]-14.0*in[tr_size-2][j]+26.0*in[tr_size-3][j]-24.0*in[tr_size-4][j]+11.0*in[tr_size-5][j]-2.0*in[tr_size-6][j])/(spacing*spacing*spacing*spacing);
				diff[tr_size-2][j] = (3.0*in[tr_size-2][j]-14.0*in[tr_size-3][j]+26.0*in[tr_size-4][j]-24.0*in[tr_size-5][j]+11.0*in[tr_size-6][j]-2.0*in[tr_size-7][j])/(spacing*spacing*spacing*spacing);

				for (int i = 2; i < tr_size-2; i++)
				{
					diff[i][j] = (in[i-2][j]-4.0*in[i-1][j]+6.0*in[i][j]-4.0*in[i+1][j]+in[i+2][j])/(spacing*spacing*spacing*spacing);
				}
			}
		}
	}
	return;
}

void gctl::difference_2d(const array<double> &in, array<double> &diff, int row_size, int col_size, 
	double spacing, gradient_type_e d_type, int order)
{
	std::string err_str;
	if (order < 1 || order > 4)
		throw invalid_argument("The input order can only be 1 to 4. Thrown by void gctl::difference_2d(...)");

	if (spacing <= 0.0)
		throw invalid_argument("The input spacing can't be negative or zero. Thrown by void gctl::difference_2d(...)");

	if (row_size*col_size != in.size())
		throw invalid_argument("The input array size does not match. Thrown by void gctl::difference_2d(...)");

	int t_size;
	if (d_type == Dx)
	{
		t_size = col_size;
	}
	else if (d_type == Dy)
	{
		t_size = row_size;
	}
	else
	{
		throw logic_error("The calculation type must be Dx or Dy. Thrown by gctl::difference_2d(...)");
	}

	if (order == 1 && t_size < 3)
		throw runtime_error("The input array size must be equal to or bigger than 3x3 for the first order derivative. Thrown by gctl::difference_2d(...)");

	if (order == 2 && t_size < 4)
		throw runtime_error("The input array size must be equal to or bigger than 4x4 for the second order derivative. Thrown by gctl::difference_2d(...)");

	if (order == 3 && t_size < 6)
		throw runtime_error("The input array size must be equal to or bigger than 6x6 for the third order derivative. Thrown by gctl::difference_2d(...)");

	if (order == 4 && t_size < 7)
		throw runtime_error("The input array size must be equal to or bigger than 7x7 for the fourth order derivative. Thrown by gctl::difference_2d(...)");

	int tr_size = row_size, tl_size = col_size;
	diff.resize(tr_size*tl_size);

	int idx;
	if (d_type == Dx)
	{
		if (order == 1)
		{
			for (int i = 0; i < tr_size; i++)
			{
				idx = i*tl_size;
				diff[idx] = (-3.0*in[idx]+4.0*in[idx+1]-in[idx+2])/(2.0*spacing);

				idx = i*tl_size + tl_size-1;
				diff[idx] = (3.0*in[idx]-4.0*in[idx-1]+in[idx-2])/(2.0*spacing);

				for (int j = 1; j < tl_size-1; j++)
				{
					idx = i*tl_size + j;
					diff[idx] = (in[idx+1] - in[idx-1])/(2.0*spacing);
				}
			}
		}
		else if (order == 2)
		{
			for (int i = 0; i < tr_size; i++)
			{
				idx = i*tl_size;
				diff[idx] = (2.0*in[idx]-5.0*in[idx+1]+4.0*in[idx+2]-in[idx+3])/(spacing*spacing);

				idx = i*tl_size + tl_size-1;
				diff[idx] = (2.0*in[idx]-5.0*in[idx-1]+4.0*in[idx-2]-in[idx-3])/(spacing*spacing);

				for (int j = 1; j < tl_size-1; j++)
				{
					idx = i*tl_size + j;
					diff[idx] = (in[idx-1]-2.0*in[idx]+in[idx+1])/(spacing*spacing);
				}
			}
		}
		else if (order == 3)
		{
			for (int i = 0; i < tr_size; i++)
			{
				idx = i*tl_size;
				diff[idx] = (-5.0*in[idx]+18.0*in[idx+1]-24.0*in[idx+2]+14.0*in[idx+3]-3.0*in[idx+4])/(2.0*spacing*spacing*spacing);

				idx = i*tl_size+1;
				diff[idx] = (-5.0*in[idx]+18.0*in[idx+1]-24.0*in[idx+2]+14.0*in[idx+3]-3.0*in[idx+4])/(2.0*spacing*spacing*spacing);

				idx = i*tl_size + tl_size-1;
				diff[idx] = (5.0*in[idx]-18.0*in[idx-1]+24.0*in[idx-2]-14.0*in[idx-3]+3.0*in[idx-4])/(2.0*spacing*spacing*spacing);

				idx = i*tl_size + tl_size-2;
				diff[idx] = (5.0*in[idx]-18.0*in[idx-1]+24.0*in[idx-2]-14.0*in[idx-3]+3.0*in[idx-4])/(2.0*spacing*spacing*spacing);

				for (int j = 2; j < tl_size-2; j++)
				{
					idx = i*tl_size + j;
					diff[idx] = (-1.0*in[idx-2]+2.0*in[idx-1]-2.0*in[idx+1]+in[idx+2])/(2.0*spacing*spacing*spacing);
				}
			}
		}
		else
		{
			for (int i = 0; i < tr_size; i++)
			{
				idx = i*tl_size;
				diff[idx] = (3.0*in[idx]-14.0*in[idx+1]+26.0*in[idx+2]-24.0*in[idx+3]+11.0*in[idx+4]-2.0*in[idx+5])/(spacing*spacing*spacing*spacing);

				idx = i*tl_size+1;
				diff[idx] = (3.0*in[idx]-14.0*in[idx+1]+26.0*in[idx+2]-24.0*in[idx+3]+11.0*in[idx+4]-2.0*in[idx+5])/(spacing*spacing*spacing*spacing);

				idx = i*tl_size + tl_size-1;
				diff[idx] = (3.0*in[idx]-14.0*in[idx-1]+26.0*in[idx-2]-24.0*in[idx-3]+11.0*in[idx-4]-2.0*in[idx-5])/(spacing*spacing*spacing*spacing);

				idx = i*tl_size + tl_size-2;
				diff[idx] = (3.0*in[idx]-14.0*in[idx-1]+26.0*in[idx-2]-24.0*in[idx-3]+11.0*in[idx-4]-2.0*in[idx-5])/(spacing*spacing*spacing*spacing);

				for (int j = 2; j < tl_size-2; j++)
				{
					idx = i*tl_size + j;
					diff[idx] = (in[idx-2]-4.0*in[idx-1]+6.0*in[idx]-4.0*in[idx+1]+in[idx+2])/(spacing*spacing*spacing*spacing);
				}
			}
		}
	}
	else
	{
		if (order == 1)
		{
			for (int j = 0; j < tl_size; j++)
			{
				idx = j;
				diff[idx] = (-3.0*in[idx]+4.0*in[idx+tl_size]-in[idx+2*tl_size])/(2.0*spacing);

				idx = (tr_size-1)*tl_size + j;
				diff[idx] = (3.0*in[idx]-4.0*in[idx-tl_size]+in[idx-2*tl_size])/(2.0*spacing);

				for (int i = 1; i < tr_size-1; i++)
				{
					idx = i*tl_size + j;
					diff[idx] = (in[idx+tl_size] - in[idx-tl_size])/(2.0*spacing);
				}
			}
		}
		else if (order == 2)
		{
			for (int j = 0; j < tl_size; j++)
			{
				idx = j;
				diff[idx] = (2.0*in[idx]-5.0*in[idx+tl_size]+4.0*in[idx+2*tl_size]-in[idx+3*tl_size])/(spacing*spacing);

				idx = (tr_size-1)*tl_size + j;
				diff[idx] = (2.0*in[idx]-5.0*in[idx-tl_size]+4.0*in[idx-2*tl_size]-in[idx-3*tl_size])/(spacing*spacing);

				for (int i = 1; i < tr_size-1; i++)
				{
					idx = i*tl_size + j;
					diff[idx] = (in[idx-tl_size]-2.0*in[idx]+in[idx+tl_size])/(spacing*spacing);
				}
			}
		}
		else if (order == 3)
		{
			for (int j = 0; j < tl_size; j++)
			{
				idx = j;
				diff[idx] = (-5.0*in[idx]+18.0*in[idx+tl_size]-24.0*in[idx+2*tl_size]+14.0*in[idx+3*tl_size]-3.0*in[idx+4*tl_size])/(2.0*spacing*spacing*spacing);

				idx = tl_size+j;
				diff[idx] = (-5.0*in[idx]+18.0*in[idx+tl_size]-24.0*in[idx+2*tl_size]+14.0*in[idx+3*tl_size]-3.0*in[idx+4*tl_size])/(2.0*spacing*spacing*spacing);

				idx = (tr_size-1)*tl_size + j;
				diff[idx] = (5.0*in[idx]-18.0*in[idx-tl_size]+24.0*in[idx-2*tl_size]-14.0*in[idx-3*tl_size]+3.0*in[idx-4*tl_size])/(2.0*spacing*spacing*spacing);

				idx = (tr_size-2)*tl_size + j;
				diff[idx] = (5.0*in[idx]-18.0*in[idx-tl_size]+24.0*in[idx-2*tl_size]-14.0*in[idx-3*tl_size]+3.0*in[idx-4*tl_size])/(2.0*spacing*spacing*spacing);

				for (int i = 2; i < tr_size-2; i++)
				{
					idx = i*tl_size + j;
					diff[idx] = (-1.0*in[idx-2*tl_size]+2.0*in[idx-tl_size]-2.0*in[idx+tl_size]+in[idx+2*tl_size])/(2.0*spacing*spacing*spacing);
				}
			}
		}
		else
		{
			for (int j = 0; j < tl_size; j++)
			{
				idx = j;
				diff[idx] = (3.0*in[idx]-14.0*in[idx+tl_size]+26.0*in[idx+2*tl_size]-24.0*in[idx+3*tl_size]+11.0*in[idx+4*tl_size]-2.0*in[idx+5*tl_size])/(spacing*spacing*spacing*spacing);

				idx = tl_size+j;
				diff[idx] = (3.0*in[idx]-14.0*in[idx+tl_size]+26.0*in[idx+2*tl_size]-24.0*in[idx+3*tl_size]+11.0*in[idx+4*tl_size]-2.0*in[idx+5*tl_size])/(spacing*spacing*spacing*spacing);

				idx = (tr_size-1)*tl_size + j;
				diff[idx] = (3.0*in[idx]-14.0*in[idx-tl_size]+26.0*in[idx-2*tl_size]-24.0*in[idx-3*tl_size]+11.0*in[idx-4*tl_size]-2.0*in[idx-5*tl_size])/(spacing*spacing*spacing*spacing);

				idx = (tr_size-2)*tl_size + j;
				diff[idx] = (3.0*in[idx]-14.0*in[idx-tl_size]+26.0*in[idx-2*tl_size]-24.0*in[idx-3*tl_size]+11.0*in[idx-4*tl_size]-2.0*in[idx-5*tl_size])/(spacing*spacing*spacing*spacing);

				for (int i = 2; i < tr_size-2; i++)
				{
					idx = i*tl_size + j;
					diff[idx] = (in[idx-2*tl_size]-4.0*in[idx-tl_size]+6.0*in[idx]-4.0*in[idx+tl_size]+in[idx+2*tl_size])/(spacing*spacing*spacing*spacing);
				}
			}
		}
	}
	return;
}
